#include "VisionControllerBW.h"

VisionControllerBW::VisionControllerBW(float horizontalTolerance,
		float verticalTolerance, int maxGhostFrames, int* epsilonFromCenter,
		int* epsilonStabilization) {
	this->camera = new VideoCapture();
	camera->open(CV_CAP_OPENNI);
	if (camera->grab()) {
		camera->retrieve(frame, CV_CAP_OPENNI_BGR_IMAGE);
	}
	this->state = ADAPTIVE_GRID;
	this->horizontalTolerance = horizontalTolerance;
	this->verticalTolerance = verticalTolerance;

	pthread_mutex_init(&mutex, 0);
	this->maxGhostFrames = maxGhostFrames;
	this->depthTrackingToleranceMin = 300;
	this->depthTrackingTolerance = DEPTHMAX - DEPTHMIN;
	this->lastMeanValue = 0;
	this->epsilonFromCenter = epsilonFromCenter;
	this->epsilonStabilization = epsilonStabilization;
	this->minTargetBottomY = frame.rows / 3;
	this->lastTargetIsValid = false;

	this->rightROIs = new vector<Rect*>();
	this->leftROIs = new vector<Rect*>();
	this->lastTarget = new Target();
//	int rightObstacleCenterX = (float) ((float) 3 / (float) 4) * frame.cols
//			+ epsilonFromCenter / 2;
//	int rightObstacleCenterY = frame.rows / 2;
	int rightObstacleCenterX = frame.cols / 2 + 100;
	int rightObstacleCenterY = frame.rows / 2;

	int rightObstacleTopLeftX = rightObstacleCenterX - obstacleROISide / 2;
	int rightObstacleTopLeftY = rightObstacleCenterY - obstacleROISide / 2;
	Rect* r1 = new Rect(rightObstacleTopLeftX, rightObstacleTopLeftY,
			obstacleROISide, obstacleROISide);
	Rect* r2 = new Rect(rightObstacleTopLeftX + obstacleROISide,
			rightObstacleTopLeftY, obstacleROISide, obstacleROISide);
	//inserisco dal pi�� interno al pi�� esterno
	rightROIs->push_back(r1);
	rightROIs->push_back(r2);

	bwPatternFiltering = new BWPatternFiltering(150);

	//int leftObstacleCenterX = (frame.cols / 2 - epsilonFromCenter) / 2;
	int leftObstacleCenterX = frame.cols / 2 - 100;

	int leftObstacleCenterY = frame.rows / 2;
	int leftObstacleTopLeftX = leftObstacleCenterX - obstacleROISide / 2;
	int leftObstacleTopLeftY = leftObstacleCenterY - obstacleROISide / 2;
	Rect* l1 = new Rect(leftObstacleTopLeftX, leftObstacleTopLeftY,
			obstacleROISide, obstacleROISide);
	Rect* l2 = new Rect(leftObstacleTopLeftX - obstacleROISide,
			leftObstacleTopLeftY, obstacleROISide, obstacleROISide);
	//inserisco dal pi�� interno al pi�� esterno
	leftROIs->push_back(l2);
	leftROIs->push_back(l1);

	int frameCenterX = frame.cols / 2;
	int frameCenterY = frame.rows / 2;
	int topLeftX = frameCenterX - obstacleROISide / 2;
	int topLeftY = frameCenterY - obstacleROISide / 2;
	centerROI = new Rect(topLeftX, topLeftY, obstacleROISide, obstacleROISide);

}

VisionControllerBW::~VisionControllerBW() {
	camera->release();
	delete camera;
	rightROIs->clear();
	delete rightROIs;
	leftROIs->clear();
	delete leftROIs;
	delete lastTarget;

	pthread_mutex_destroy(&mutex);
}

Rect VisionControllerBW::getNearestTarget(vector<Rect>& markers) {
	uint16_t minDistance = DEPTHMAX;
	int minIndex = 0;

	for (int i = 0; i < markers.size(); i++) {
		uint16_t dist = getObjectDistance(markers.at(i), false);
		//se il marker non �� compreso in un range di tolleranza in profondit��
		// lo scarto impostando la sua distanza a DEPTHMAX
//		cout << "lastTargetDistance: " << lastTarget->getDistance() << endl;
//		cout << "tolerance: " << depthTrackingTolerance << endl;
		if (abs(dist - lastTarget->getDistance()) > depthTrackingTolerance) {
			//cout << "Scartato" << endl;

			dist = DEPTHMAX;
		}

		if (minDistance > dist) {
			minDistance = dist;
			minIndex = i;
		}
	}

	// nel caso non sia sopravvissuto alcun marker valido restituisco il marker vuoto
	if (minDistance == DEPTHMAX)
		return Rect(0, 0, 0, 0);
	else
		return markers.at(minIndex);
}

// 0 se il bersaglio sta al centro
// 1 se il bersaglio �� a sinistra del centro
// -1 se il bersaglio �� a destra del centro
Target* VisionControllerBW::processFrame() {
	//Sezione critica
	Target* target = NULL;
	try {

		pthread_mutex_lock(&mutex);

		//Fotogramma attuale dalla camera
		if (camera->grab()) {
			camera->retrieve(frame, CV_CAP_OPENNI_BGR_IMAGE);
			camera->retrieve(depthMap, CV_CAP_OPENNI_DEPTH_MAP);

			target = searchTarget();

			//restituisco un target trovato solo se lo trovo per almeno
			//due frame consecutivi
			if (!lastTargetIsValid) {
				this->lastTargetIsValid = target->isValid();
				target->setDistance((uint16_t) 0);
				target->setDistanceFromCenter(0);
				target->setGhostFrames(0);

			} else
				this->lastTargetIsValid = target->isValid();

			//aggiorno il last target e gestisco il ghost
			if (target->isValid()) {

				*lastTarget = *target;
			} else if (lastTarget->getGhostFrames() < maxGhostFrames) {
				//target non trovato, utilizzo il ghost per maxGhostFrames

				lastTarget->setGhostFrames(lastTarget->getGhostFrames() + 1);

				*target = *lastTarget;

			}
			// Marker ghost scaduto
			else {
				*lastTarget = *target;

			}

			//cout << target.getGhostFrames() << endl;
			int x = frame.cols / 2 + target->getDistanceFromCenter();
			line(frame, Point(x, 0), Point(x, frame.rows), Scalar(255, 0, 0),
					3);
			//VISUALIZZAZIONE
			drawLineEpsilon(*epsilonFromCenter);
			drawLineEpsilon(*epsilonStabilization);

			rectangle(frame, *centerROI, Scalar(255, 255, 255), 3);
			for (int i = 0; i < rightROIs->size(); i++) {
				rectangle(frame, *rightROIs->at(i), Scalar(255, 255, 255), 3);
				rectangle(frame, *leftROIs->at(i), Scalar(255, 255, 255), 3);
				stringstream leftDistanceString;
				leftDistanceString << getObjectDistance(*leftROIs->at(i), true)
						<< endl;
				stringstream rightDistanceString;
				rightDistanceString
						<< getObjectDistance(*rightROIs->at(i), true) << endl;
				putText(frame, leftDistanceString.str(),
						Point(leftROIs->at(i)->x, 50 + i * 50),
						FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 255, 0), 2, 8);
				putText(frame, rightDistanceString.str(),
						Point(rightROIs->at(i)->x, 50 + i * 50),
						FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 255, 0), 2, 8);
			}

			stringstream centerDistanceString;
			centerDistanceString << getFrontalDistance() << endl;

			putText(frame, centerDistanceString.str(), Point(centerROI->x, 50),
					FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 255, 0), 2, 8);

			imshow("BGR", frame);
			waitKey(1);

		}
	} catch (...) {
		cout << "eccezione processFrame" << endl;

	}

	//cout << "lastTargetIsValid: " << lastTargetIsValid << endl;
	pthread_mutex_unlock(&mutex);
	if (target == NULL)
		target = new Target();
	return target;
}

Target* VisionControllerBW::searchTarget() {

	vector<Rect> connectedComponents;
	Target* target;

	Mat thresh;

	bwPatternFiltering->apply(frame, thresh);

	//Componenti connesse
	vector<vector<Point> > contours;
	Mat tempCopy;
	thresh.copyTo(tempCopy);
	findContours(tempCopy, contours, noArray(), CV_RETR_EXTERNAL,
			CV_CHAIN_APPROX_SIMPLE);
	for (int k = 0; k < contours.size(); k++) {
		Rect r = boundingRect(contours.at(k));
		//if (r.height >= 20 && r.width >= 20)
		connectedComponents.push_back(r);
		//rectangle(frame, r, Scalar(127 * i, 127 * i, 127 * i), 4);
	}

	targetDetection(connectedComponents);

	Rect targetROI(0, 0, 0, 0);
	if (connectedComponents.size() > 0)
		targetROI = getNearestTarget(connectedComponents);

	target = new Target();

	//selezione del marker finale
	if (targetROI.width > 0 && targetROI.height > 0) {
		uint16_t depthDistance = getObjectDistance(targetROI, false);
		int centerX = targetROI.x + targetROI.width / 2;
		int distanceFromCenter = centerX - frame.cols / 2;
		target->setDistanceFromCenter(distanceFromCenter);
		target->setDistance(depthDistance);
	}

	depthTrackingTolerance = updateDepthTrackingTolerance();

	return target;
}

uint16_t VisionControllerBW::getObjectDistance(cv::Rect objectROI,
		bool useOnlyDepthMap) {
	vector<uint16_t> distanceValues;

	uint16_t distance = 0;

	//imshow("depth", depthMap);

	Mat region(this->depthMap, objectROI);
	for (int y = 0; y < region.rows; y++) {
		for (int x = 0; x < region.cols; x++) {
			distanceValues.push_back(region.at<uint16_t>(y, x));
		}
	}

	distance = getMedianValue(distanceValues);
	uint16_t visionDistance = calculateDistanceFromDimensions(objectROI);
	//se il valore fornito �� nullo, cerchiamo di stimare la distanza
	//esclusivamente tramite la visione

	if (!useOnlyDepthMap && visionDistance > 0 && visionDistance < DEPTHMIN) {
		distance = visionDistance;

	}

	return distance;
}

uint16_t VisionControllerBW::getMedianValue(vector<uint16_t> values) {
	set<uint16_t> container;
	for (int i = 0; i < values.size(); i++) {
		container.insert(values.at(i));
	}
	set<uint16_t>::iterator it = container.begin();

	advance(it, container.size() / 2);
	//cout << (*it) << endl;

	return (*it);
}

void VisionControllerBW::targetDetection(vector<Rect>& connectedComponents) {

	//scarta componenti sottilissime in larghezza oppure
	//troppo troppo piccole in altezza oppure
	//troppo in cielo
	vector<Rect>::iterator it = connectedComponents.begin();
	while (it != connectedComponents.end()) {
		Rect r = (*it);
		if (r.width < minTargetWidth || r.height < minTargetHeight
				|| (r.y + r.height) < minTargetBottomY) {
			it = connectedComponents.erase(it);
		} else
			++it;
	}
}

uint16_t VisionControllerBW::calculateDistanceFromDimensions(
		cv::Rect objectROI) {

	//Distanza massima 70cm
	double height_mexp = 0.99581971;
	double height_bexp = 122.664501;
	int distanceEstimation = height_bexp * pow(height_mexp, objectROI.height);
	if (distanceEstimation > 0 && distanceEstimation < 70)
		return distanceEstimation * 10;		//distanza in mm
	else
		return 0;

}

uint16_t VisionControllerBW::getFrontalDistance() {

	return getObjectDistance(*centerROI, true);
}

uint16_t VisionControllerBW::updateDepthTrackingTolerance() {
	uint16_t tolerance;
	if (lastTarget->getDistance() == 0)
		tolerance = DEPTHMAX - DEPTHMIN;
	else
		tolerance = depthTrackingToleranceMin
				+ lastTarget->getGhostFrames()
						* (DEPTHMAX - DEPTHMIN - depthTrackingToleranceMin)
						/ maxGhostFrames;

	return tolerance;
}

void VisionControllerBW::drawLineEpsilon(int epsilon) {
	line(frame, Point(frame.cols / 2 - epsilon, 0),
			Point(frame.cols / 2 - epsilon, frame.rows - 1),
			Scalar(255, 255, 0), 3);
	line(frame, Point(frame.cols / 2 + epsilon, 0),
			Point(frame.cols / 2 + epsilon, frame.rows - 1),
			Scalar(255, 255, 0), 3);

}

vector<uint16_t> *VisionControllerBW::getRightObstaclesDistances() {
	return getObstaclesDistances(rightROIs);
}

vector<uint16_t> *VisionControllerBW::getLeftObstaclesDistances() {
	return getObstaclesDistances(leftROIs);
}

vector<uint16_t>* VisionControllerBW::getObstaclesDistances(
		vector<Rect*>* ROIs) {
	vector<uint16_t>* result = new vector<uint16_t>();
	for (int i = 0; i < ROIs->size(); i++) {
		uint16_t distance = getObjectDistance(*(ROIs->at(i)), true);
		if (distance > 0)
			result->push_back(distance);
		else
			result->push_back((uint16_t) DEPTHMAX);
	}
	return result;
}
